#include "../User/Device/CMD.h"

#include "../BSP/uart.h"
#include "main.h"

void CMD_Init(CMD_Send_t *cmd) {
  BSP_Uart_Init();
  cmd->operation = CMD_OPERATION_NOTHING;
  cmd->data = 0.0f;
}

bool CMD_Update(CMD_Send_t *cmd) {
  if (BSP_Uart_CheckFlag()) {
    CMD_PraseCommand(cmd, BSP_Uart_GetDataAddr());
    return true;
  } else {
    cmd->operation = CMD_OPERATION_NOTHING;
    return false;
  }
}

void CMD_Apply(CMD_Send_t *cmd, PID_Param_t *pid_angle,
               PID_Param_t *pid_speed) {
  switch (cmd->operation) {
    case CMD_OPERATION_ANGLE_K:
      pid_angle->advantage.k = cmd->data;
      break;
    case CMD_OPERATION_ANGLE_P:
      pid_angle->p = cmd->data;
      break;
    case CMD_OPERATION_ANGLE_I:
      pid_angle->i = cmd->data;
      break;
    case CMD_OPERATION_ANGLE_D:
      pid_angle->d = cmd->data;
      break;
    case CMD_OPERATION_SPEED_K:
      pid_speed->advantage.k = cmd->data;
      break;
    case CMD_OPERATION_SPEED_P:
      pid_speed->p = cmd->data;
      break;
    case CMD_OPERATION_SPEED_I:
      pid_speed->i = cmd->data;
      break;
    case CMD_OPERATION_SPEED_D:
      pid_speed->d = cmd->data;
      break;
    default:
#ifdef DEBUG
      while (1) __NOP();
#else
      return;
#endif
  }
}
